home *** CD-ROM | disk | FTP | other *** search
/ Games of Daze / Infomagic - Games of Daze (Summer 1995) (Disc 1 of 2).iso / x2ftp / msdos / progsrc / impact21 / impact.c < prev    next >
Encoding:
C/C++ Source or Header  |  1995-01-27  |  28.8 KB  |  913 lines
  1. /*
  2.   IMPACT - Virtual Reality Workshop
  3.     First implementation by John Henckel on Oct 7, 1993
  4.  
  5.    Copyright (c) 1993,1995 John Henckel
  6.    Permission to use, copy, modify, distribute and sell this software
  7.    and its documentation for any purpose is hereby granted without fee,
  8.    provided that the above copyright notice appear in all copies.
  9.  
  10.   Acknowledgements:
  11.   * This program was developed using Borland Turbo C++ 3.0.  The speed and
  12.     versatility of the graphics of this program is a tribute to the talented
  13.     programmers at Borland.  Also their online help is fabulous.
  14.   * The equations for collision of irregularly shaped objects are taken from
  15.     "Dynamics" by Pestel and Thomson, 1968, I haven't found such a detailed
  16.     analysis of this problem in any other book.
  17.  
  18.   Comments on ver 2.0 -- complex machines
  19.     I wanted to have machines with movable parts and limbs.  The approach
  20.   I've taken is this.  Every machine must have one root part, all other
  21.   parts refer to this root.  A sub (non-root) is connected to either the
  22.   root or another sub by a joint.  The joint determines the position
  23.   and velocity of a sub.  A part can only be connected to a body prior
  24.   to it in the array of bodies, thus a root has a lower index than
  25.   all its subparts.  The user has interactive control over the machine
  26.   my changing the jvx,jvy,jw -- the velocity of the joints.
  27.  
  28.   Note on 1/26/95 by john....
  29.   As you may be able to tell, this code is in transition.  I tried very
  30.   hard to fit the joint mechanism into an existing simple body design.
  31.   However, I have given up.  The part about conserving rotational inertia
  32.   while a joint is moving stumped me.  Someday I would like to start over
  33.   using c++ classes and bitmaps and real joints.
  34.  
  35.   Uploaded to Simtel 1/27/95.
  36. */
  37. #include <stdlib.h>
  38. #include <stdio.h>
  39. #include <string.h>
  40. #include <time.h>
  41. #include <math.h>
  42. #include <graphics.h>
  43. #include <dos.h>
  44. #include <conio.h>
  45. #include "mouse.h"
  46.  
  47. // Macros and constants
  48.  
  49. #define MA 10       // array size
  50. #define MB 50       // bodies
  51. #define MX 40        // virtual x dimension size (origin lower-left)
  52. #define MY 30        // virtual y size
  53. #define EPS 0.00001 // small number
  54. #define PI 3.1415926
  55. #define BEEP 2000    // two bodies hit
  56. #define BLIP 1000    // body hit wall
  57. #define frand(x) (x/1000.0*random(1000))
  58. #define rline(a,b,c,d) line((a)*xm/MX,ym-(b)*ym/MY,(c)*xm/MX,ym-(d)*ym/MY)
  59.  
  60. //  The following are used for timing parts of the code
  61. #define tini memset(timer,0,sizeof timer)
  62. #define tion(x) timer[x].c = clock()
  63. #define tiof(x) timer[x].a += clock()-timer[x].c
  64. #define tisw(x) tiof(x); tion(x+1)
  65.  
  66. // Types
  67.  
  68. typedef float fa[MA];
  69. typedef struct { int x,y; } pt;
  70.  
  71. typedef struct {     // Body:       (- means scratch field)
  72.   int color;         //  color of the body
  73.   float r;           //  radius (maximum)
  74.   float m;           //  mass
  75.   float i;           //  coefficient of moment of inertia ( = I/m )
  76.   int n;             //  number of points in the shape (0=ball,1=??)
  77.   fa sx,sy;          //  shape (center of mass is always origin)
  78.   fa px,py;          //- corner points
  79.   fa len;            //  length of each edge (from i to i+1)
  80.   pt poly[2][MA+1];  //- polygons for draw and erase
  81.   float vx,vy,w;     //  velocity
  82.   float cx,cy,a;     //  center position
  83.   float ocx,ocy,oa;  //- old center position
  84.   int rb;            //  root body index (must be less than my index)
  85.   int jb;            //  joint body (less than mine) <or> num parts
  86.   float jx,jy,ja;    //  location of pivot
  87.   float jvx,jvy,jw;  //  velocity of pivot
  88.   float kx,ky;       //  my offset to pivot <or> abs tot center of mass
  89.   float tm,ti;       //- total mass and moment coeff
  90. } body;
  91.  
  92. // Global Variables
  93.  
  94. int xm,ym;
  95. body bod[MB];        // Array of bodies
  96. int numb;
  97. int wall=15;
  98. struct { clock_t a,c; } timer[10];
  99.  
  100. // Global options
  101.  
  102. float fric=0.0;   // air friction
  103. float grav=0.0;   // gravity
  104. float scut=0.9;   // collision center adjustment
  105. float rest=1.0;   // coefficient of restitution
  106. int trace=0;      // erase image on redraw
  107. int clear=0;      // clear screen
  108. int mous=0;       // mouse present?
  109. int delet=0;      // delete last body
  110. int noisy=0;      // turn on sound
  111. int debug=0;
  112. int cycle=0;       // color cycling
  113. float covel=0.01;  // maximum contact velocity
  114. float slid=0;       // sliding friction
  115.  
  116. /*----------------------------------------------------------------------
  117.    Redraw_all - erase all bodies and redraw them in the new position
  118. */
  119. int redraw_all(int p)
  120. {
  121.   int b;
  122.  
  123.   if (clear) {
  124.     cleardevice();
  125.     wall=15;
  126.     clear=0;
  127.   }
  128.   for (b=0; b<numb; ++b) {
  129.     setcolor(0);                         // black
  130.     if (!trace) drawpoly(bod[b].n+1,(int*)bod[b].poly[1-p]);
  131.     if (b<numb-delet) {
  132.       if (cycle) bod[b].color = (bod[b].color+1)&255;
  133.       setcolor(bod[b].color);
  134.       drawpoly(bod[b].n+1,(int*)bod[b].poly[p]);
  135.     }
  136.   }
  137.   numb -= delet;
  138.   if (numb<0) numb=0;
  139.   delet=0;
  140.  
  141.   setcolor(1);
  142.   if (wall&1) line(0,0,xm,0);
  143.   if (wall&2) line(xm,0,xm,ym);
  144.   if (wall&4) line(xm,ym,0,ym);
  145.   if (wall&8) line(0,ym,0,0);
  146.   wall=0;
  147.  
  148.   return 0;
  149. }
  150.  
  151. /*----------------------------------------------------------------------
  152.    draw mouse pointer
  153. */
  154. int draw_mouse(int x, int y, int c)
  155. {
  156.   setcolor(c);
  157.   line(x+5,y+5,x-5,y-5);
  158.   line(x-5,y+5,x+5,y-5);
  159.   return 0;
  160. }
  161.  
  162. /*-----------------------------------------------------------------
  163.    add new bodies - see help message in get_input
  164. */
  165. void addnew(char *s)
  166. {
  167.   int i,j,k,c,n,x;
  168.   float d,t,x1,y1,a,m;
  169.  
  170.   n=1; j=4; d=1; x1=1; c=0; y1=0.0001; m=0; a=0;   // defaults
  171.   memset(&bod[numb],0,sizeof *bod);
  172.   bod[numb].cx = bod[numb].cy = 0.5;
  173.   bod[numb].rb = numb;
  174.   sscanf(s,"%d %d %f %f %d %f %f %f %f %f %f %f %f %f"
  175.            " %d %d %f %f %f %f %f %f %f %f",
  176.          &n,&j,&x1,&d,&c,&y1,&m,&a,
  177.          &bod[numb].cx,&bod[numb].cy,&bod[numb].a,
  178.          &bod[numb].vx,&bod[numb].vy,&bod[numb].w,
  179.          &bod[numb].rb,&bod[numb].jb,
  180.          &bod[numb].jx,&bod[numb].jy,&bod[numb].ja,
  181.          &bod[numb].jvx,&bod[numb].jvy,&bod[numb].jw,
  182.          &bod[numb].kx,&bod[numb].ky);
  183.   if (d<EPS) d=0.1;
  184.   if (d>2) d=2;
  185.   if (m==0) m=PI*d*d*x1*100;
  186.   if (a==0) a=d*d*x1;
  187.   if (j<2) j=2;
  188.   if (j>=MA) j=MA-1;
  189.   if (n<0) n=0;
  190.   if (n+numb > MB) n=MB-numb;
  191.   if (y1>1) y1=1;
  192.  
  193.   // Set up the first new body
  194.   bod[numb].color = c;
  195.   bod[numb].r = d*x1;
  196.   bod[numb].m = m;
  197.   bod[numb].i = a;
  198.  
  199.   for (i=numb; i<numb+n; ++i) {
  200.     bod[i] = bod[numb];
  201.     bod[i].rb = i-numb+bod[numb].rb;      // ??
  202.     if (c==0) bod[i].color=9+i%6;
  203.     if (i>numb) {
  204.       bod[i].cy = bod[i-1].cy + 2*d;
  205.       bod[i].cx = bod[i-1].cx;
  206.     }
  207.     if (bod[i].cy+d > MY) {
  208.       bod[i].cy = 0.5;
  209.       bod[i].cx = bod[i-1].cx + 2*d*x1;
  210.     }
  211.     bod[i].n = j;
  212.     for (k=0; k<j; ++k) {
  213.       bod[i].sx[k] = d*(1-frand(y1))*cos(PI*2*(k+0.5+frand(y1))/j)*-x1;
  214.       bod[i].sy[k] = d*(1-frand(y1))*sin(PI*2*(k+0.5+frand(y1))/j);
  215.     }
  216.     for (k=0; k<bod[i].n; ++k) {
  217.       x = k+1; if (x==bod[i].n) x=0;
  218.       bod[i].len[k] = hypot(bod[i].sx[k]-bod[i].sx[x],
  219.                             bod[i].sy[k]-bod[i].sy[x]);
  220.       if (bod[i].len[k] < EPS) bod[i].len[k] = EPS;
  221.     }
  222.     bod[i].w += frand(PI*y1);
  223.   }
  224.   numb += n;
  225. }
  226.  
  227. /*----------------------------------------------------------------------
  228.    help - display help
  229. */
  230. int help(void)
  231. {
  232.   gotoxy(1,1);
  233.   printf("IMPACT Dynamics Simulator.  Version 2.1 ("__DATE__")\n");
  234.   printf(" by John Henckel, henckel@vnet.ibm.com\n\n");
  235.   printf("Press a to add a new bodies\n");
  236.   printf("Press c to clear screen\n");
  237.   printf("Press d to delete the last body\n");
  238.   printf("Press e for an energy report\n");
  239.   printf("Press f to change air friction [0.0]               ");
  240.   printf("[] = default\n");
  241.   printf("Press g to change gravitation constant [0.0]\n");
  242.   printf("Press j to change body center adjustment factor [.9]\n");
  243.   printf("Press q to toggle sound\n");
  244.   printf("Press r to change restitution coefficient [1.0]\n");
  245.   printf("Press s to change sliding friction [0]\n");
  246.   printf("Press t to toggle trace mode [0]\n");
  247.   printf("Press v to change sliding threshold vel [.01]\n");
  248.   printf("Press x to toggle debug mode [0]\n");
  249.   printf("Press y to cycle colors\n");
  250.   printf("Press ? or F1 for help\n");
  251.   if (mous) {
  252.     printf("You can use the mouse to pull things.\n");
  253.     printf("  button 1=pull, button 2=stop spin, both=pull ALL!\n");
  254.   }
  255.   printf("\nPress a key to continue\n");
  256.   while (!kbhit());
  257.   return clear=1;
  258. }
  259.  
  260. /*----------------------------------------------------------------------
  261.    msg - display a message on the message line
  262. */
  263. int msg(int c,char *s)
  264. {
  265.   gotoxy(5,c+2);
  266.   printf("%s",s);
  267.   return clear=1;
  268. }
  269. int msgd(int c,char *s,int d)
  270. {
  271.   char x[80];
  272.   sprintf(x,"%-30s %d",s,d);
  273.   return msg(c,x);
  274. }
  275. int msgf(int c,char *s,float d)
  276. {
  277.   char x[80];
  278.   sprintf(x,"%-30s %9.7f",s,d);
  279.   return msg(c,x);
  280. }
  281.  
  282.  
  283. /*----------------------------------------------------------------------
  284.    get_input  and redraw mouse pointer
  285. */
  286. #define VV       0.1
  287. int get_input(void)
  288. {
  289.   static int b=-1,x=0,y=0;
  290.   static char s[320];
  291.   int i,j,k,c,n;
  292.   float d,t,x1,y1,a,m;
  293.  
  294.   nosound();
  295.   if (!numb) {
  296.     help();
  297.     cleardevice();
  298.   }
  299.   if (mous) {
  300.     draw_mouse(x,y,0);
  301.     b=get_mouse(&x,&y);
  302.     draw_mouse(x,y,15-b);
  303.   } else b=0;
  304.   if (b) {
  305.     x1 = 1.0*x/xm*MX;
  306.     y1 = (1-1.0*y/ym)*MY;
  307.     if (b<3) {
  308.       d = 99999;                //  find closest body
  309.       for (j=i=0; i<numb; ++i) {
  310.         t = hypot(x1-bod[i].cx,y1-bod[i].cy);
  311.         if (t < d) { d=t; j=i; }
  312.       }
  313.       if (d > EPS) {
  314.         bod[j].vx = (x1-bod[j].cx)*VV;
  315.         bod[j].vy = (y1-bod[j].cy)*VV;
  316.       }
  317.       if (b>1) bod[j].w = 0;           // stop spin
  318.     }
  319.     else {                                // attract everything
  320.       for (j=0; j<numb; ++j) {
  321.         bod[j].vx = (x1-bod[j].cx)*VV*0.1;
  322.         bod[j].vy = (y1-bod[j].cy)*VV*0.1;
  323.       }
  324.     }
  325.   }
  326.   if (kbhit()) {
  327.     while (kbhit()) b=getch();      // skip over typeahead and extended codes
  328.     if (b==27) return 1;
  329.     if (b=='g') {
  330.       msg(0,"Enter gravitation constant*10000 (-10.0 to 10.0): ");
  331.       scanf("%f",&grav);
  332.       grav /= 10000;
  333.     }
  334.     if (b=='f') {
  335.       msg(0,"Enter air friction constant (0.0-100.0): ");
  336.       scanf("%f",&fric);
  337.     }
  338.     if (b=='r') {
  339.       msg(0,"Enter coefficient of restitution (0.0 - 1.0): ");
  340.       scanf("%f",&rest);
  341.       if (rest<-1) rest=-1;
  342.       if (rest>2)  rest=2;
  343.     }
  344.     if (b=='v') {
  345.       msg(0,"Enter sliding impact threshold (0.0 - 0.1): ");
  346.       scanf("%f",&covel);
  347.     }
  348.     if (b=='s') {
  349.       msg(0,"Enter sliding friction (0.0 - 1.0): ");
  350.       scanf("%f",&slid);
  351.       if (slid< -0.9) slid=-0.9;
  352.     }
  353.     if (b=='e') {
  354.       t=a=d=m=0;
  355.       for (i=0; i<numb; ++i) {
  356.         t += 0.5*bod[i].m*(bod[i].vx*bod[i].vx + bod[i].vy*bod[i].vy);
  357.         a += 0.5*bod[i].m*bod[i].i*bod[i].w*bod[i].w;
  358.         d += bod[i].m*bod[i].cy*grav;
  359.       }
  360.       m=t+a+d;
  361.       msgd(0,"Total number of bodies",numb);
  362.       msgf(1,"Total translation energy",t);
  363.       msgf(2,"Total rotation energy",a);
  364.       msgf(3,"Total potential energy",d);
  365.       msgf(4,"Total energy",m);
  366.       msgf(6,"Gravitation",grav);
  367.       msgf(7,"Friction",fric);
  368.       msgf(8,"Restitution coefficient",rest);
  369.       msgf(9,"Collision adjustment",scut);
  370.       msgf(10,"Sliding threshold vel",covel);
  371.       msgf(11,"Sliding friction",slid);
  372.       msgd(12,"Mouse detected",mous);
  373.       msgd(13,"Debug mode",debug);
  374.       while (!kbhit());
  375.     }
  376.     if (b=='j') {
  377.       msg(0,"Enter collision center adjustment (0.0 - 1.0): ");
  378.       scanf("%f",&scut);
  379.       if (scut<-1) scut=-1;
  380.       if (scut>2)  scut=2;
  381.     }
  382.     if (b=='q') noisy^=1;
  383.     if (b=='y') cycle^=1;
  384.     if (b=='d') delet=1;
  385.     if (b=='a' && numb<MB) {      // Add new body
  386.       msg(0,"Enter data: Count[1] Sides[4] Aspect[1] Radius[1] Color[0?]");
  387.       msg(1,"            Noise[0] Mass[0] Moment[0] Pos[.5 .5 0] Vel[0 0 0]");
  388.       msg(2,"[] = default.  Default mass and moment is based on radius, "
  389.           "unit density.");
  390.       msg(3,"For example, for five rectangles type: 5 4 2\n");
  391.       do { gets(s); } while(*s < ' ');
  392.       addnew(s);
  393.     }
  394.     if (b=='t') trace^= 1;
  395.     if (b=='c') clear = 1;
  396.     if (b=='x') debug = 1;
  397.     if (b=='?' || b==';') help();
  398.   }
  399.   return 0;
  400. }
  401.  
  402. /*----------------------------------------------------------------------
  403.    computes new c,a  +=  new v,w
  404. */
  405. int compute_new(void)
  406. {
  407.   int b;
  408.   float sa,ca;
  409.   float sx,sy;
  410.   for (b=0; b<numb; ++b)
  411.     if (bod[b].rb < b) {      // a sub part
  412.       bod[b].jx += bod[b].jvx;
  413.       bod[b].jy += bod[b].jvy;
  414.       bod[b].ja += bod[b].jw;
  415.     }
  416.     else {                   // a root part
  417.       bod[b].cx += bod[b].vx + (bod[b].ky - bod[b].cy)*bod[b].w;
  418.       bod[b].cy += bod[b].vy + (bod[b].cx - bod[b].kx)*bod[b].w;
  419.       bod[b].a  += bod[b].w;
  420.     }
  421.   return 0;
  422. }
  423.  
  424. /*----------------------------------------------------------------------
  425.    sets px,py (corners) based on shape and cx,cy (center), a (angle)
  426. */
  427. int compute_pxy(void)
  428. {
  429.   int i,b;
  430.   float cx,cy,ca,sa;
  431.  
  432.   for (b=0; b<numb; ++b) {
  433.     cx = bod[b].cx;
  434.     cy = bod[b].cy;
  435.     ca = cos(bod[b].a);
  436.     sa = sin(bod[b].a);
  437.  
  438.     for (i=0; i<bod[b].n; ++i) {
  439.       bod[b].px[i] = cx + bod[b].sx[i]*ca - bod[b].sy[i]*sa;
  440.       bod[b].py[i] = cy + bod[b].sx[i]*sa + bod[b].sy[i]*ca;
  441.     }
  442.   }
  443.   return 0;
  444. }
  445.  
  446. /*----------------------------------------------------------------------
  447.    set_old  - copy new c,a,v,w to old.
  448. */
  449. int set_old(void)
  450. {
  451.   int b;
  452.   for (b=0; b<numb; ++b) {
  453.     bod[b].ocx = bod[b].cx;
  454.     bod[b].ocy = bod[b].cy;
  455.     bod[b].oa  = bod[b].a ;
  456.   }
  457.   return 0;
  458. }
  459.  
  460. /*----------------------------------------------------------------------
  461.    copy px,py to poly[p],  adjust for scale.
  462.    the range of pxy is [MX,MY], poly is [xm,ym] y inverted.
  463. */
  464. int set_poly(int p)
  465. {
  466.   int i,b;
  467.  
  468.   for (b=0; b<numb; ++b) {
  469.     for (i=0; i<bod[b].n; ++i) {
  470.       bod[b].poly[p][i].x = bod[b].px[i]*xm/MX;
  471.       bod[b].poly[p][i].y = (MY-bod[b].py[i])*ym/MY;
  472.     }
  473.     bod[b].poly[p][i].x = bod[b].poly[p][0].x;      // close the polygon
  474.     bod[b].poly[p][i].y = bod[b].poly[p][0].y;
  475.   }
  476.   return 0;
  477. }
  478.  
  479. /*----------------------------------------------------------------------
  480.    distance from a point to a line (the edge of another body).
  481.    result is negative inside (clockwise).
  482. */
  483. float dptl(float x,float y,int b,int i)
  484. {
  485.   int t;
  486.   float nx,ny;
  487.   t = i+1;
  488.   if (t==bod[b].n) t=0;
  489.   nx = bod[b].py[i] - bod[b].py[t];
  490.   ny = bod[b].px[t] - bod[b].px[i];    // n is normal vector
  491.   x -= bod[b].px[i];
  492.   y -= bod[b].py[i];
  493.   nx = (x*nx + y*ny) / bod[b].len[i];
  494.   return nx;
  495. }
  496.  
  497. /*-----------------------------------------------------------------
  498.    adjust machines - this routine adjusts
  499.    1. the roots of every machine by setting: center of mass, total mass
  500.       and inertia, num bodies
  501.    2. the subparts of every machine by setting: position, velocity
  502. */
  503. int adjust_parts() {
  504.   int b,r,j;
  505.   float ca,sa,px,py,rx,ry;
  506.  
  507.   for (b=0; b<numb; ++b) {
  508.     r = bod[b].rb;
  509.     if (r>=b) {
  510.       bod[b].jb = 0;         // num sub parts
  511.       bod[b].kx = bod[b].cx;
  512.       bod[b].ky = bod[b].cy;
  513.       bod[b].tm = bod[b].m;
  514.       bod[b].ti = bod[b].i*bod[b].m;
  515.     }
  516.     else {
  517.       j=bod[b].jb;
  518.       if (j<r || j>=b) bod[b].jb=j=r;    //  require r <= j < b
  519.       /*-----------------------------------------------------------------
  520.          figure out the position of b
  521.       */
  522.       ca = cos(bod[j].a);
  523.       sa = sin(bod[j].a);
  524.       px = bod[j].cx + ca*bod[b].jx - sa*bod[b].jy;
  525.       py = bod[j].cy + ca*bod[b].jy + sa*bod[b].jx;
  526. //  setcolor(15);  rline(bod[j].cx,bod[j].cy,px,py);
  527.       bod[b].a = bod[j].a + bod[b].ja;
  528.       ca = cos(bod[b].a);
  529.       sa = sin(bod[b].a);
  530.       rx = ca*bod[b].kx - sa*bod[b].ky;
  531.       ry = ca*bod[b].ky + sa*bod[b].kx;
  532.       if (debug)
  533.         ca=1;
  534.       bod[b].cx = px - rx;
  535.       bod[b].cy = py - ry;
  536. //  setcolor(15);  rline(bod[b].cx,bod[b].cy,px,py);  for debugging joints
  537.       /*-----------------------------------------------------------------
  538.          figure out the velocity of b
  539.       */
  540.       px = bod[b].cx - bod[j].cx;
  541.       py = bod[b].cy - bod[j].cy;
  542.       bod[b].vx = bod[j].vx + bod[b].jvx - py*bod[j].w + ry*bod[b].jw;
  543.       bod[b].vy = bod[j].vy + bod[b].jvy + px*bod[j].w + rx*bod[b].jw;
  544.       bod[b].w = bod[j].w + bod[b].jw;
  545.       /*-----------------------------------------------------------------
  546.          update root totals
  547.       */
  548.       ++bod[r].jb;
  549.       px = bod[r].tm + bod[b].m;
  550.       bod[r].kx = (bod[r].kx*bod[r].tm + bod[b].cx*bod[b].m) / px;
  551.       bod[r].ky = (bod[r].ky*bod[r].tm + bod[b].cy*bod[b].m) / px;
  552.       bod[r].tm = px;
  553.       px = bod[r].cx - bod[j].cx;
  554.       py = bod[r].cy - bod[j].cy;
  555.       bod[r].ti += bod[b].m*(px*px + py*py);      //  I = mr^2
  556.     }
  557.   }
  558.   return 0;
  559. }
  560. /*-----------------------------------------------------------------
  561.    collision - test for a collision between two bodies, if so,
  562.    return the point of impact, and unit vector normal to surface
  563.    of impact (points into b2), and penetration distance.
  564. */
  565. int collision(int b1,int b2,
  566.               int *B1,int *B2,
  567.               float *hx,float *hy,
  568.               float *nx,float *ny,float *pen)
  569. {
  570.   float d1,d2,dd,d,px,py;
  571.   int i,j,i1,i2,k1,k2,t=0;
  572.   /*-----------------------------------------------------------------
  573.      If both bodies are part of the same machine, ignore collision
  574.   */
  575.   if (bod[b1].rb==bod[b2].rb) return 0;
  576.   /*-----------------------------------------------------------------
  577.      The following is for efficiency.  If the centers are far apart,
  578.      then don't test for collision.
  579.   */
  580.   d1 = fabs(bod[b1].cx - bod[b2].cx);
  581.   d2 = fabs(bod[b1].cy - bod[b2].cy);
  582.   d = bod[b1].r + bod[b2].r;
  583.   if (d1 > d || d2 > d) return 0;
  584.   /*-----------------------------------------------------------------
  585.      First test for corners of b1 inside b2, then b2 inside b1
  586.   */
  587.   dd = 0;
  588.   while (++t < 3) {
  589.     for (i=0; i<bod[b1].n; ++i) {
  590.       px = bod[b1].px[i];
  591.       py = bod[b1].py[i];
  592.       d1 = d2 = -99999;
  593.       for (j=0; j<bod[b2].n; ++j) {
  594.         d = dptl(px,py,b2,j);
  595.         if (d > EPS) break;             // not inside
  596.         if (d > d1) {
  597.           d2=d1; i2=i1;        //  d2 is second best
  598.           d1=d;  i1=j;         //  d1 is dist to nearest edge
  599.         }
  600.         else if (d > d2) {
  601.           d2=d;  i2=j;         //  d2 is second best
  602.         }
  603.       }
  604.       /*-----------------------------------------------------------------
  605.          The following bit of code is usually not necessary, usually
  606.          we can bounce corner i off of edge i1.  However, sometimes the
  607.          correct edge is the second best, i2.  This is when the corners
  608.          adjacent to i, k1 and k2, are not farther from edge i1 than i.
  609.       */
  610.       k1 = i-1;    if (k1<0) k1 = bod[b1].n-1;
  611.       k2 = i+1;    if (k2==bod[b1].n) k2 = 0;
  612.       if (j==bod[b2].n && j>2 &&
  613.           (d1 > dptl(bod[b1].px[k1],bod[b1].py[k1],b2,i1) ||
  614.            d1 > dptl(bod[b1].px[k2],bod[b1].py[k2],b2,i1)) &&
  615.           d2 < dptl(bod[b1].px[k1],bod[b1].py[k1],b2,i2) &&
  616.           d2 < dptl(bod[b1].px[k2],bod[b1].py[k2],b2,i2)) {
  617.         i1 = i2;
  618.         d1 = d2;
  619.       }
  620.       /*-----------------------------------------------------------------
  621.          Store parameters for this bounce
  622.       */
  623.       if (j==bod[b2].n && d1<dd) {    // maximum penetration
  624.         dd = d1;
  625.         i2 = i1+1;                // i2 = other end point of the edge
  626.         if (i2==bod[b2].n) i2=0;
  627.         *ny = (bod[b2].px[i1] - bod[b2].px[i2])/bod[b2].len[i1];
  628.         *nx = (bod[b2].py[i2] - bod[b2].py[i1])/bod[b2].len[i1];
  629.         *hx = px;        // + *nx*d1;
  630.         *hy = py;        // + *ny*d1;
  631.         *B1 = b1;
  632.         *B2 = b2;
  633.         *pen = -d1;
  634.       }
  635.     }
  636.     i = b1; b1 = b2; b2 = i;            // swap b1 and b2
  637.   }
  638.   if (dd == 0) return 0;       // no collision found between these bodies
  639.   if (debug) {
  640.     setcolor(15);
  641.     rline(*hx,*hy,*hx+*nx/10,*hy+*ny/10);
  642.     while (!kbhit());                // wait for keypress
  643.     i=getch();
  644.     if (i=='x') debug=0;
  645.   }
  646.   if (noisy) sound(BEEP);
  647.   return 1;
  648. }
  649.  
  650. /*----------------------------------------------------------------------
  651.    get_accel  - get any changes to velocity from collisions, gravity, etc
  652. */
  653. int get_accel(void)
  654. {
  655.   int t,t1,t2;
  656.   int i1,b1,b2,r;
  657.   static float x1,x2,y1,y2,v1,v2,c1,c2,w1,w2,m1,m2,o1,o2;
  658.   static float nx,ny,z,a1,a2,a,b,c,hx,hy,ri;
  659.  
  660.   // test if any body bumped into a wall
  661.  
  662.   for (b1=0; b1<numb; ++b1) {
  663.     x1=MX; x2=0;
  664.     for (i1=0; i1<bod[b1].n; ++i1) {
  665.       if (x1 > bod[b1].px[i1]) {        // find leftmost
  666.         x1=bod[b1].px[i1];
  667.         y1=bod[b1].py[i1];
  668.       }
  669.       if (x2 < bod[b1].px[i1]) {        // find rightmost
  670.         x2=bod[b1].px[i1];
  671.         y2=bod[b1].py[i1];
  672.       }
  673.     }
  674.     if (x2 > MX) { x1=x2-MX; y1=y2; wall|=2; }
  675.     else if (x1 > 0) x1=0;
  676.     else wall|=8;
  677.     if (x1) {
  678.       if (debug) {
  679.         setcolor(15);
  680.         a = x1<0 ? 0 : MX-.1;
  681.         rline(a,y1,a+.1,y1);
  682.         while (!kbhit());                // wait for keypress
  683.         if (getch()=='x') debug=0;
  684.       }
  685.       if (noisy) sound(BLIP);
  686.       r = bod[b1].rb;           // root of machine
  687.       ri = bod[r].ti/bod[r].tm;
  688.       y1 = bod[r].ky - y1;
  689.       w1 = bod[b1].w;           // ??? b1 or r ???
  690.       v1 = bod[b1].vx;
  691.       a = y1*y1/ri;
  692.       c1 = ((a-rest)*v1 - y1*(1+rest)*w1)/(a+1);
  693.       bod[r].vx = c1;
  694.       bod[r].w = y1*(c1 - v1)/ri + w1;    // - jw ??
  695.       bod[r].cx -= scut*x1;
  696.     }
  697.     y1=MY; y2=0;
  698.     for (i1=0; i1<bod[b1].n; ++i1) {
  699.       if (y1 > bod[b1].py[i1]) {        // find bottommost
  700.         x1=bod[b1].px[i1];
  701.         y1=bod[b1].py[i1];
  702.       }
  703.       if (y2 < bod[b1].py[i1]) {        // find topmost
  704.         x2=bod[b1].px[i1];
  705.         y2=bod[b1].py[i1];
  706.       }
  707.     }
  708.     if (y2 > MY) { y1=y2-MY; x1=x2; wall|=1; }
  709.     else if (y1 > 0) y1=0;
  710.     else wall|=4;
  711.     y2=y1;
  712.     if (y2) {
  713.       if (debug) {
  714.         setcolor(15);
  715.         a = y2<0 ? 0 : MY-.1;
  716.         rline(x1,a,x1,a+.1);
  717.         while (!kbhit());                // wait for keypress
  718.         if (getch()=='x') debug=0;
  719.       }
  720.       if (noisy) sound(BLIP);
  721.       r = bod[b1].rb;           // root of machine
  722.       ri = bod[r].ti/bod[r].tm;
  723.       y1 = -bod[r].kx + x1;
  724.       w1 = bod[b1].w;
  725.       v1 = bod[b1].vy;
  726.       a = y1*y1/ri;
  727.       c1 = ((a-rest)*v1 - y1*(1+rest)*w1)/(a+1);
  728.       bod[r].vy = c1;
  729.       bod[r].w = y1*(c1 - v1)/ri + w1;   // - jw ??
  730.       bod[r].cy -= scut*y2;
  731.       /*----------------------------------------------------------
  732.          Do sliding friction on the floor (or ceiling)
  733.       */
  734.       z = bod[b1].vx;              // relative sliding velocity
  735.       z *= 1-1/(slid+1);
  736.       bod[b1].vx -= z;
  737.     }
  738.   }
  739.   //  For every pair of shapes, test if any of b1's corners are inside b2.
  740.  
  741.   for (t1=0; t1<numb; ++t1)
  742.     for (t2=t1+1; t2<numb; ++t2)
  743.       if (collision(t1,t2,&b1,&b2,&hx,&hy,&nx,&ny,&z)) {
  744.         // Prepare to compute collision
  745.       r = bod[b1].rb;           // root of machine
  746.       ri = bod[r].ti/bod[r].tm;
  747.       r = bod[b1].rb;           // root of machine
  748.       ri = bod[r].ti/bod[r].tm;
  749.         v1 = bod[b1].vx*nx + bod[b1].vy*ny;
  750.         v2 = bod[b2].vx*nx + bod[b2].vy*ny;
  751.         y1 = (hx-bod[b1].ocx)*ny - (hy-bod[b1].ocy)*nx;
  752.         y2 = (hx-bod[b2].ocx)*ny - (hy-bod[b2].ocy)*nx;
  753.         w1 = bod[b1].w;
  754.         w2 = bod[b2].w;
  755.         m1 = bod[b1].m;
  756.         m2 = bod[b2].m;
  757.         a1 = bod[b1].i;
  758.         a2 = bod[b2].i;
  759.         /*--------------------------------------------------------------------
  760.            find change in velocity using "Dynamics" impact equations
  761.         */
  762.         a = (a1*m1*y2*y2 + a2*m2*y1*y1)/(a1*a2);
  763.         b = m1 + m2;
  764.         if (b < EPS) break;           // mass is too small
  765.         c = y1*w1 - y2*w2;
  766.         c1 = ((a + m1 - rest*m2)*v1 + (1+rest)*m2*(v2 - c)) / (a + b);
  767.         c2 = ((a + m2 - rest*m1)*v2 + (1+rest)*m1*(v1 + c)) / (a + b);
  768.         o1 = y1*(c1 - v1)/a1;
  769.         o2 = y2*(c2 - v2)/a2;
  770.         bod[b1].w += o1;
  771.         bod[b2].w += o2;
  772.         bod[b1].vx += nx*(c1 - v1);
  773.         bod[b1].vy += ny*(c1 - v1);
  774.         bod[b2].vx += nx*(c2 - v2);
  775.         bod[b2].vy += ny*(c2 - v2);
  776.         /*--------------------------------------------------------------------
  777.            adjust the centers of the two bodies so they aren't overlapped,
  778.            movement is inversely proportional to mass.
  779.         */
  780.         z *= scut/b;
  781.         bod[b1].cx -= nx*z*m2;
  782.         bod[b1].cy -= ny*z*m2;
  783.         bod[b2].cx += nx*z*m1;
  784.         bod[b2].cy += ny*z*m1;
  785.         /*-----------------------------------------------------------------
  786.            Compute sliding friction
  787.         */
  788.         a = v1 - v2;         //  approach velocity
  789.         if (fabs(a) < covel) {
  790.           c1 = bod[b1].vy*nx - bod[b1].vx*ny;
  791.           c2 = bod[b2].vy*nx - bod[b2].vx*ny;
  792.           z = c1 - c2;              // relative sliding velocity
  793.           z *= 1-1/(slid+1);
  794.           z /= b;
  795.           bod[b1].vx += ny*z*m2;
  796.           bod[b1].vy -= nx*z*m2;
  797.           bod[b2].vx -= ny*z*m1;
  798.           bod[b2].vy += nx*z*m1;
  799.         }
  800.       }
  801.  
  802.   //   Add air friction and gravity
  803.   //   and check for centers out of range
  804.  
  805.   for (b1=0; b1<numb; ++b1) {
  806.     z = hypot(bod[b1].vx,bod[b1].vy);
  807.     z = 1/(1+z*fric);                // air friction
  808.     bod[b1].vy *= z;
  809.     bod[b1].vx *= z;
  810.     bod[b1].vy -= grav;             // gravity
  811.  
  812.     t = 0;
  813.     if (bod[b1].cx < 0) { t=1;  bod[b1].cx = 0; }
  814.     if (bod[b1].cx >MX) { t=1;  bod[b1].cx = MX; }
  815.     if (bod[b1].cy < 0) { t=1;  bod[b1].cy = 0; }
  816.     if (bod[b1].cy >MY) { t=1;  bod[b1].cy = MY; }
  817.     if (t) {
  818.       bod[b1].vx *= 0.9;          // slow it down!
  819.       bod[b1].vy *= 0.9;
  820.       bod[b1].w *= 0.9;
  821.     }
  822.   }
  823.   return 0;
  824. }
  825. /*----------------------------------------------------------------------
  826.    Animate
  827.    This is the main action loop of the program.
  828.    On entry these must be set.
  829.      bod, numb, xm, ym
  830.    and graphics screen must be up and clear.
  831. */
  832. int animate(void)
  833. {
  834.   int p=0;
  835.  
  836.   adjust_parts();          // set subpart new c,a
  837.   compute_pxy();           // compute corners based on new c,a
  838.   set_poly(p);             // initialize old poly
  839.  
  840.   while (1) {
  841.     p = 1-p;          tion(0);
  842.     if (get_input()) break; tisw(0); // test mouse and keyboard
  843.     set_old();        tisw(1);     // set old c,a to new ones.
  844.     compute_new();    tisw(2);     // new c,a = old c,a + v,w
  845.     adjust_parts();                // set subpart new c,a
  846.     compute_pxy();    tisw(3);     // compute corners based on new c,a
  847.     set_poly(p);      tisw(4);     // set poly based on corners
  848.     redraw_all(p);    tisw(5);     // erase and draw all bodies
  849.     set_old();        tisw(6);     // set old c,a to new ones.     ???
  850.     get_accel();      tiof(7);     // test for any collisions, etc
  851.   }
  852.   return 0;
  853. }
  854.  
  855. /*-----------------------------------------------------------------
  856.    readfile
  857. */
  858. void readfile(char *filename)
  859. {
  860.   char s[256];
  861.   FILE *f;
  862.   f = fopen(filename,"r");
  863.   if (f==NULL) return;
  864.   fgets(s,sizeof s,f);
  865.   sscanf(s,"%f %f %f %f %d %f %f",&fric,&grav,&scut,&rest,&trace,&covel,&slid);
  866.   while (1) {
  867.     if (!fgets(s,sizeof s,f)) break;
  868.     if (*s>30) addnew(s);
  869.   }
  870.   fclose(f);
  871. }
  872.  
  873. /*----------------------------------------------------------------------
  874.    M A I N
  875. */
  876. int main(int argc, char **argv)
  877. {
  878.   int i,gm,gd=0;
  879.  
  880.   printf("IMPACT Dynamics Simulator.  Version 2.1 ("__DATE__")\n");
  881.   printf(" by John Henckel, henckel@vnet.ibm.com\n\n");
  882.   printf("Copyright (c) 1993,1995 John Henckel\n");
  883.   printf("Permission to use, copy, modify, distribute and sell this software\n");
  884.   printf("and its documentation for any purpose is hereby granted without fee,\n");
  885.   printf("provided that the above copyright notice appear in all copies.\n");
  886.   if (argc<2) {
  887.   printf("\nParms: filename -mono\n\n");
  888.   printf("If you have EGA/VGA, then do not specify -mono.\n");
  889.   printf("If you have CGA, Hercules, or other .bgi type, specify -mono.\n");
  890.   i=10; while (!kbhit() && --i) delay(500);
  891.   }
  892.   if (argc<3)
  893.     registerbgidriver(EGAVGA_driver);  // omit this line to use .BGI file
  894.   initgraph(&gd,&gm,"");
  895.   srand(time(NULL));
  896.   if (graphresult() != grOk) return
  897.     printf("\nUnable to initialize graphics adapter\n");
  898.   xm = getmaxx();
  899.   ym = getmaxy();
  900.   if (init_mouse()==-1) {
  901.     mous = 1;
  902.     hide_mouse();              // we make our own pointer.
  903.   }
  904.   numb = 0;
  905.   if (argc>1) readfile(argv[1]);
  906.   tini;
  907.   animate();
  908.   closegraph();
  909.   for (gm=0; gm<10; ++gm)
  910.     if (timer[gm].a) printf("timer %d is %10d\n",gm,timer[gm].a);
  911.   return 0;
  912. }
  913.